/*
 *  This is the SmartCar Standing Code;
 *  Copyright (C) 2011-2012 WenX, SEU, P.R.China <wenxiao1992@gmail.com>
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#ifndef WENX_CALCDIS_H
#define WENX_CALCDIS_H

#include "const.h"
//PS4 Trig PS5 ECHO
//PS6 Trig PS7 ECHO


void initcalcspd(){
  DDRS_DDRS0=1;
  DDRS_DDRS2=1;
  DDRS_DDRS1=0;
  DDRS_DDRS3=0;  
}

unsigned int test_ct,countp,countn;
int Degree_Distance;
int Delta_Degree_Distance;
void calcdegree(){
     int i;
     test_ct++;
     

       PTS_PTS0=1;
       for(i=0;i<1000;i++)
          ;
       PTS_PTS0=0;
       for(countp=0;countp<1000&&PTS_PTS1==0;countp++)
          ;
       for(countn=0;countn<7000&&PTS_PTS1==1;countn++)
       
          ;
       if(NOTREADY()&&((countn-Degree_Distance>1000)||(countn-Degree_Distance<1000))){





        ChangeLED(0,1);
       }else
        countn=(countn+Degree_Distance)/2;
        Delta_Degree_Distance=(countn)-Degree_Distance;
        Degree_Distance=(countn); 
       
        //NumToShow=Degree_Distance;
}




#endif